
#define BOOST_TEST_MODULE MMM_MOTION_TEST

#include "MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h"
#include "MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h"
#include "MMM/Motion/Motion.h"
#include "MMM/MathTools.h"
#include "TestHelper.h"

#include <boost/test/unit_test.hpp>

#include <iostream>

BOOST_AUTO_TEST_CASE( AllMeasurementsForTimestep )
{

}

BOOST_AUTO_TEST_CASE( SegmentMotion )
{

}

BOOST_AUTO_TEST_CASE( AddSensor )
{
    // Setup Motion
    MMM::ModelPtr mockupModel = TestHelper::createMockupModel(std::set<std::string> {"BLNx_joint", "BLNy_joint", "BRNx_joint", "ARNx_joint", "BRNy_joint", "ARNy_joint"});
    MMM::MotionPtr motion(new MMM::Motion("Motion", mockupModel, mockupModel));

    // Test nullpointer
    BOOST_REQUIRE_THROW(motion->addSensor(nullptr), MMM::Exception::MMMException);

    MMM::KinematicSensorPtr kSensor1(new MMM::KinematicSensor(std::vector<std::string> {"BLNx_joint", "BLNy_joint"}));
    MMM::KinematicSensorMeasurementPtr kSensorMeasurement(new MMM::KinematicSensorMeasurement(0.0, Eigen::Vector2f(1.0f, 1.0f)));
    kSensor1->addSensorMeasurement(kSensorMeasurement);
    kSensor1->setName("K");
    BOOST_REQUIRE_NO_THROW(motion->addSensor(kSensor1));
    BOOST_REQUIRE_NO_THROW(motion->getSensorByName("K"));
    BOOST_REQUIRE_NO_THROW(motion->getSensorByName("K")->getMeasurement(0.0));
    BOOST_REQUIRE_THROW(motion->addSensor(kSensor1), MMM::Exception::MMMException);; // Already a sensor with the same config in the motion

    MMM::KinematicSensorPtr kSensor2(new MMM::KinematicSensor(std::vector<std::string> {"BLNx_joint", "BLNy_joint", "BLNz_joint"}));
    BOOST_REQUIRE_THROW(motion->addSensor(kSensor2), MMM::Exception::MMMException);; // Not matching Model

    MMM::KinematicSensorPtr kSensor3(new MMM::KinematicSensor(std::vector<std::string> {"BRNx_joint", "ARNx_joint"}));
    kSensor3->addSensorMeasurement(kSensorMeasurement);
    kSensor3->setName("K");
    BOOST_REQUIRE_NO_THROW(motion->addSensor(kSensor3));
    BOOST_REQUIRE(motion->getSensorByName("K") != kSensor3); // Already a sensor with the same name in the motion
    BOOST_REQUIRE(motion->getSensorByName("Kinematic"));

    MMM::KinematicSensorPtr kSensor4(new MMM::KinematicSensor(std::vector<std::string> {"BRNy_joint", "ARNy_joint"}));
    kSensor4->addSensorMeasurement(kSensorMeasurement);
    BOOST_REQUIRE_NO_THROW(motion->addSensor(kSensor4, 1.0));
    BOOST_REQUIRE(motion->getSensorByName("Kinematic2"));
    BOOST_REQUIRE(motion->getSensorByName("Kinematic2")->getMeasurement(1.0));

    MMM::ModelPoseSensorPtr mSensor(new MMM::ModelPoseSensor());
    MMM::ModelPoseSensorMeasurementPtr mSensorMeasurement(new MMM::ModelPoseSensorMeasurement(0.0, Eigen::Vector3f(2.0f, 0.0f, 1.0f), Eigen::Vector3f(2.0f, 0.0f, 1.0f)));
    mSensor->addSensorMeasurement(mSensorMeasurement);
    BOOST_REQUIRE_NO_THROW(motion->addSensor(mSensor, -1.0));
    BOOST_REQUIRE(motion->getSensorByName("K")->getMeasurement(1.0)); // Is now synchronized
}

BOOST_AUTO_TEST_CASE( JoinMotion )
{
    // Setup Model
    MMM::ModelPtr mockupModel = TestHelper::createMockupModel(std::set<std::string> {"BLNx_joint", "BLNy_joint", "BRNx_joint", "BRNy_joint"});

    // Setup needed kinematic sensor measurements
    MMM::KinematicSensorMeasurementPtr kSensorMeasurement1(new MMM::KinematicSensorMeasurement(0.0, Eigen::Vector2f(1.0f, 1.0f)));
    MMM::KinematicSensorMeasurementPtr kSensorMeasurement2(new MMM::KinematicSensorMeasurement(1.0, Eigen::Vector2f(1.0f, 2.0f)));
    MMM::KinematicSensorMeasurementPtr kSensorMeasurement3(new MMM::KinematicSensorMeasurement(3.0, Eigen::Vector2f(3.0f, 1.0f)));
    MMM::KinematicSensorMeasurementPtr kSensorMeasurement4(new MMM::KinematicSensorMeasurement(0.0, Eigen::Vector2f(5.0f, 5.0f)));

    // Setup motion 1
    MMM::MotionPtr motion1(new MMM::Motion("Motion", mockupModel, mockupModel));
    MMM::KinematicSensorPtr m1_kSensor(new MMM::KinematicSensor(std::vector<std::string> {"BLNx_joint", "BLNy_joint"}));
    m1_kSensor->addSensorMeasurement(kSensorMeasurement1);
    m1_kSensor->addSensorMeasurement(kSensorMeasurement3);
    BOOST_REQUIRE_NO_THROW(motion1->addSensor(m1_kSensor));
    MMM::ModelPoseSensorPtr mSensor(new MMM::ModelPoseSensor());
    MMM::ModelPoseSensorMeasurementPtr mSensorMeasurement(new MMM::ModelPoseSensorMeasurement(0.0, Eigen::Vector3f(2.0f, 0.0f, 1.0f), Eigen::Vector3f(2.0f, 0.0f, 1.0f)));
    mSensor->addSensorMeasurement(mSensorMeasurement);
    BOOST_REQUIRE_NO_THROW(motion1->addSensor(mSensor));

    // Setup motion 2
    MMM::MotionPtr motion2(new MMM::Motion("Motion2", mockupModel, mockupModel));
    MMM::KinematicSensorPtr m2_kSensor1(new MMM::KinematicSensor(std::vector<std::string> {"BLNx_joint", "BLNy_joint"}));
    m2_kSensor1->addSensorMeasurement(kSensorMeasurement2);
    m2_kSensor1->addSensorMeasurement(kSensorMeasurement3);
    BOOST_REQUIRE_NO_THROW(motion2->addSensor(m2_kSensor1));
    MMM::KinematicSensorPtr m2_kSensor2(new MMM::KinematicSensor(std::vector<std::string> {"BRNx_joint", "BRNy_joint"}));
    m2_kSensor2->addSensorMeasurement(kSensorMeasurement1);
    m2_kSensor2->addSensorMeasurement(kSensorMeasurement2);
    BOOST_REQUIRE_NO_THROW(motion2->addSensor(m2_kSensor2));

    // Join Motion successfull
    MMM::MotionPtr joinedMotion = MMM::Motion::join(motion1, motion2);
    BOOST_REQUIRE(joinedMotion);
    BOOST_REQUIRE(joinedMotion->getSensorData().size() == 3);
    BOOST_REQUIRE(joinedMotion->getSensorByName("ModelPose"));
    BOOST_REQUIRE(joinedMotion->getSensorsByType("Kinematic").size() == 2);
    MMM::SensorPtr sensor = joinedMotion->getSensorByName("Kinematic");
    BOOST_REQUIRE(sensor->getMeasurement(0.0)->equals(kSensorMeasurement1));
    BOOST_REQUIRE(sensor->getMeasurement(1.0)->equals(kSensorMeasurement2));
    BOOST_REQUIRE(sensor->getMeasurement(3.0)->equals(kSensorMeasurement3));

    // Join Motion not successfull
    m2_kSensor1->addSensorMeasurement(kSensorMeasurement4);
    BOOST_REQUIRE(!MMM::Motion::join(motion1, motion2));
}

BOOST_AUTO_TEST_CASE( MinMaxTimestep )
{
    // Setup Motion
    MMM::ModelPtr mockupModel = TestHelper::createMockupModel(std::set<std::string> {"BLNx_joint", "BLNy_joint", "BRNx_joint", "BRNy_joint"});
    MMM::MotionPtr motion(new MMM::Motion("Motion", mockupModel, mockupModel));

    // Setup sensors
    MMM::KinematicSensorPtr kSensor1(new MMM::KinematicSensor(std::vector<std::string> {"BLNx_joint", "BLNy_joint"}));
    MMM::KinematicSensorMeasurementPtr kSensorMeasurement1(new MMM::KinematicSensorMeasurement(1.0, Eigen::Vector2f(1.0f, 1.0f)));
    kSensor1->addSensorMeasurement(kSensorMeasurement1);
    MMM::KinematicSensorMeasurementPtr kSensorMeasurement2(new MMM::KinematicSensorMeasurement(3.0, Eigen::Vector2f(1.0f, 1.0f)));
    kSensor1->addSensorMeasurement(kSensorMeasurement2);
    BOOST_REQUIRE_NO_THROW(motion->addSensor(kSensor1));

    MMM::KinematicSensorPtr kSensor2(new MMM::KinematicSensor(std::vector<std::string> {"BRNx_joint", "BRNy_joint"}));
    MMM::KinematicSensorMeasurementPtr kSensorMeasurement3(new MMM::KinematicSensorMeasurement(4.0, Eigen::Vector2f(1.0f, 1.0f)));
    kSensor2->addSensorMeasurement(kSensorMeasurement3);
    BOOST_REQUIRE_NO_THROW(motion->addSensor(kSensor2));

    MMM::ModelPoseSensorPtr mSensor(new MMM::ModelPoseSensor());
    MMM::ModelPoseSensorMeasurementPtr mSensorMeasurement(new MMM::ModelPoseSensorMeasurement(2.0, Eigen::Vector3f(2.0f, 0.0f, 1.0f), Eigen::Vector3f(2.0f, 0.0f, 1.0f)));
    mSensor->addSensorMeasurement(mSensorMeasurement);
    BOOST_REQUIRE_NO_THROW(motion->addSensor(mSensor));

    // Test minTimestep
    BOOST_REQUIRE(MMM::Math::equal(motion->getMinTimestep(), 1.0));

    // Test maxTimestep
    BOOST_REQUIRE(MMM::Math::equal(motion->getMaxTimestep(), 4.0));
}

// BOOST_AUTO_TEST_SUITE_END()
